#ifndef FUNCTIONS_H
#define FUNCTIONS_H
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/segment_differences.h>

using namespace std;
using namespace pcl;


int getMilliCount();

int getMilliSpan(int nTimeStart);

template< class PointT >
float getRadius(PointCloud<PointT> &cloud)
{
    Eigen::Vector4f centroid;
    compute3DCentroid(cloud,centroid);
    float maxRadius=0;
    for (int i=0; i<cloud.size();i++)
    {
        float radius_ith = ( centroid - cloud.at(i).getVector4fMap() ).norm();
        if (radius_ith>maxRadius) maxRadius=radius_ith;
    }
    return maxRadius;
}

template< class PointT >
vector<int> sphereIndices(PointCloud<PointT> &source , PointT center, float radius)
{
    boost::shared_ptr<PointCloud<PointT> > cloudPtr /*=source.makeShared();*/(new PointCloud<PointT>); copyPointCloud(source,*cloudPtr);
    search::KdTree<PointT> tree;
    tree.setInputCloud(cloudPtr);
    vector<int> indices;
    vector<float> distances;
    tree.radiusSearch(center,radius,indices,distances);
    return indices;
}


template< class PointT >
void cutShapeAroundTarget(PointCloud<PointT> &world , PointCloud<PointT> &object)
{
    float radius=getRadius(object)*0.3f;
    boost::shared_ptr< PointCloud<PointT> > worldPtr = world.makeShared();
    boost::shared_ptr< PointCloud<PointT> > objectPtr = object.makeShared();
    PointCloud<PointT> objectVoxels;
    PointCloud<PointT> newWorld;
    KdTreeFLANN<PointT> tree;
    tree.setInputCloud(worldPtr);
    vector<int> indices;
    vector<int> indicesInShape;
    vector<float> sqr_distances;
    VoxelGrid<PointT> vox;
    vox.setInputCloud(objectPtr);
    vox.setLeafSize(radius,radius,radius);
    vox.filter(objectVoxels);
    for (int i=0;i<objectVoxels.size();i++)
    {//inserisco tutte le sfere (con doppioni) in indicesInShape
        PointT currentCenter=objectVoxels.at(i);
        tree.radiusSearch(currentCenter,radius,indices,sqr_distances);
        for (int j=0;j<indices.size();j++) indicesInShape.push_back(indices.at(j));
    }
    sort(indicesInShape.begin(),indicesInShape.end());
    indices=indicesInShape;
    indicesInShape.clear();
    indicesInShape.push_back(indices.back());
    while(!indices.empty())
    { //tolgo i doppioni
        if (indicesInShape.back()==indices.back())
            indices.pop_back();
        else
            indicesInShape.push_back(indices.back());
    }
    for (int j=0;j<indicesInShape.size();j++) newWorld.push_back(world.at(indicesInShape.at(j)));
    world=newWorld;
}


template< class PointT >
vector<PointIndices> findClusters(PointCloud<PointT> &cloud, double threshold, int minClusterSize, int maxClusterSize=0)
{
    //cout << "Find Clusters... "<<flush;
    boost::shared_ptr< PointCloud<PointT> > cloudPtr = cloud.makeShared();
    vector<PointIndices> cluster_indices_out;
    EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance (threshold);
    ec.setMinClusterSize (minClusterSize);
    if (maxClusterSize<minClusterSize)
        ec.setMaxClusterSize (cloud.size());
    else
        ec.setMaxClusterSize (maxClusterSize);
    ec.setInputCloud (cloudPtr);
    ec.extract (cluster_indices_out);
    //cout <<"OK! Clusters found: " << cluster_indices_out.size()<<endl;
    return cluster_indices_out;
}


float computeLeafSize (PointCloud<PointXYZ>::Ptr cloud, int nVoxelsRequired);


template< class PointT >
void onlyNormals(PointCloud<PointT> &cloud  ,   PointCloud<Normal> &normals , double radius)
{
    //cout << "Normals estimation... " << flush;
    NormalEstimationOMP<PointT, Normal> normal_estimation;
    normal_estimation.setSearchMethod (boost::shared_ptr<search::Search<PointT> > (new search::KdTree<PointT>));
    normal_estimation.setRadiusSearch (radius);
    boost::shared_ptr<PointCloud<PointT> > cloudPtr (new PointCloud<PointT>);
    copyPointCloud(cloud,*cloudPtr);
    normal_estimation.setInputCloud (cloudPtr);
    normal_estimation.compute (normals);// *
    //cout << "OK! Normals found: " << normals->points.size() << endl;
}

#endif
